/**
 * 
 */
package SRTPlanner;

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.lang.Integer;
import java.util.ArrayList;
import java.util.HashSet;

import simulation.ConfigGraph;
import simulation.Configuration;
import simulation.Map;
import simulation.Path;
import simulation.Rect;
import simulation.Simulation;

/**
 * @author aclayton
 *
 */
public class ThreadedSRT extends Thread{
	private final int totalSamples = 40000;
	
	private Simulation sim;
	private Rect subMap;
	private ConfigGraph graph;
	private int numSamples;
	
	
	
	public ThreadedSRT(Simulation sim, Rect submap, int numSamples){
		super();
		this.sim = sim;
		subMap = submap;
		this.numSamples = numSamples;
	}

	public void run() {
		Configuration start = Configuration.CreateRandomFreeConfig(sim, subMap);
		
		EST e = new EST(sim, subMap, start);
		
		if(subMap.pointInBox(sim.getStart().getConfigCenter()))
			e.addToTree(sim.getStart());
		if(subMap.pointInBox(sim.getGoal().getConfigCenter()))
			e.addToTree(sim.getGoal());
		
		e.addRandomConfigurations(numSamples);
		graph = e.getGraph();
		
	}
	
	public ConfigGraph getConfigGraph(){
		return graph;
	}
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		
		if(args.length != 3){
			System.out.println("ThreadedSRT numThreads SimulationFile OutputPathFile");
			System.exit(0);
		}
		
		final int totalSamples = 30000;
		
		int numThreads = Integer.parseInt(args[0]);
		
		Simulation sim = new Simulation(5,5);
		sim.LoadFromFile(args[1]);
		
		long start = System.currentTimeMillis();
		
		Map map = sim.getMap();
		
		ArrayList<ThreadedSRT> threads = new ArrayList<ThreadedSRT>();
		HashSet<Rect> submaps = map.getBoundingBox().splitIntoNum(numThreads); 
		
		
		for(Rect r : submaps){
				
			
			ThreadedSRT temp = new ThreadedSRT(sim, r, totalSamples/numThreads);
			threads.add(temp);
			temp.start();
		}
		
		for(ThreadedSRT temp : threads){
			try{
				temp.join();
			} catch(Exception e) {}
		}
		
		
		// Run SRT on subgraphs
		SRT s = new SRT(sim);
		for(ThreadedSRT temp : threads){
			ConfigGraph tempGraph = temp.getConfigGraph();
			s.addGraph(tempGraph);
		}
		s.runSRT();
		ConfigGraph srtGraph = s.getSRTGraph();
		
		// Run a-star on srtGraph to find path
		Path path = srtGraph.getAStarPath(sim.getStart(), sim.getGoal());
		
		long end = System.currentTimeMillis();
		System.out.println("Time = " + (end-start));

		if(path != null){
			try{
				FileWriter fstream = new FileWriter(args[2]);
				BufferedWriter out = new BufferedWriter(fstream);
				out.write(path.toString());
				out.close();
			} catch(Exception e){
				e.printStackTrace();
			}
		} else {
			System.out.println("No path found");
		}
	}

}
